import time
from Enet import Enet, UData,YData
from datetime import datetime
import sys




# 导入自定义函数和CANFD SDK动态链接库>>>
from os.path import dirname, join
import sys
sys.path.append(join(dirname(__file__), "src"))
sys.path.append(dirname(__file__))
from src import *


# Initialize Enet
enet = Enet()
enet.ReceiveY()
now=datetime.now()




loopPeriod=0.001
cnt=0

time.sleep(1)


site_force = np.zeros(6)
filtered_site_force = np.zeros(6)

try:
    
    loopStartTime=time.time()
    while True:
        
        cnt+=1
        start_time = time.time()
        t=start_time-loopStartTime

        # 2.1 ReadY from YPool
        start_time = time.time()

        y_data = enet.ReadY()
        
        q_real_degree = np.array(y_data.pos)
        q_real_int = trans_q_degree2intq(q_real_degree)
        
        q_real_float = trans_intq2q(q_real_int)
        eP_D = np.array([.5, -.5, 1])
        eD_quat = np.array([0, 0, 1, 0])
        
        site_force_raw = np.array(y_data.F)
        
        filtered_site_force = alpha * site_force_raw + (1 - alpha) * filtered_site_force
        site_force = filtered_site_force.copy()
        print(f"site force is {site_force}")
        
        ikq_star = ika_admittance(q_real_float, eP_D, eD_quat, site_force)

        intikq = trans_q2intq(ikq_star)
        ctrl_intq = get_interpoint(q_real_int, intikq, max_dq)
        print(f"ctrl_intq = {ctrl_intq}")
        ctrl_q_degree = trans_intq2q_degree(ctrl_intq).tolist()
        print(f"ik+admittance time cost: {time.time() - start_time}")

        # 2.2 TODO:Compute U from Y
        ts = int(time.time() * 1_000_000)  # current timestamp from epoch (microsecond)
        u_data = UData(timestamp=ts, pos=ctrl_q_degree)

        # 2.3 Send U to remoteIP
        enet.SendU(u_data)

        # Sleep to maintain 1kHz loop
        elapsed_time = time.time() - start_time
        # print(elapsed_time)
        time.sleep(max(0, loopPeriod - elapsed_time))
except KeyboardInterrupt:
    print("Exiting...")